
from Brain.vector import Vector
from arm import Arm 
from Brain.lookup_table import LookupTable
import time
import math
from threading import Thread 


_rad90 = math.pi / 2.0 
BEGIN_X, BEGIN_Y, BEGIN_Z = 15 , 0, 10

TIME_SPEED_TABLE = {
	123: 35,
	82 : 25,
	52 : 18,
	39 : 15,
	35 : 14,
	31 : 13,
	28 : 12,
	23 : 11,
	19 : 10,
	}

time_speed_lookuptable = LookupTable(TIME_SPEED_TABLE)

class ArmControl(Arm, Thread):
	
	def __init__(self, world, camera):
		Thread.__init__(self)
		Arm.__init__(self)
		
		self._world = world 
		self._camera = camera 
		
		## current position 
		self._x, self._y, self._z = 10.0, 0.0, 5.0 
		## last sent position 
		self._previous_x, self._previous_y, self._previous_z = 10.0, 0.0, 5.0 
		
		## build state :
		self._task = None 
		self.pack_left = False
		self.pack_right = False
		self.pack_gripper = False
		self.pack_floor_sorage = False
		self.stored = 0 
		
		self.sensor_floor_sorage = False
		self.sensor_gripper = False

	TOLERANCE = 0.05
	DISTANCE = 1.0
	SPEED = 12
	INTERVAL_PER_REVOLUTION = 4.0
	OFFSET_X = 10.0
	
	TOP_ALTITUDE = 13.5
	
	def build_on(self, x, y, z):
		""" already holding one column element,
			place it on x,y,z
			go back to original position 
			gripper is OPEN now
		"""
		
		print' ZBUDz '
		self.move(15.0 ,0.0, 10.0)
		self._x, self._y, self._z = 15.0, 0.0, 10.0,
		time.sleep(0.5)
	
		self.record_reset()
		self.vertical_motion(self.TOP_ALTITUDE, x, y, z)
		recorded_steps = self.get_history()
		self.execute_operation('release pack')
		print 'release ..	'
		#print self.operations['release pack']
		time.sleep(2.5)
		self.reverse_motion( recorded_steps )
	
	def reverse_motion(self, history):
		index = len(history) - 1 
		last_ts = None
		while index > 0:
			move_element = history[index]
			duration = 0.0
			if last_ts:
				duration = abs(move_element.time_stamp - last_ts)
			last_ts = move_element.time_stamp
			self.write(move_element.joint_number, move_element.angle, move_element.speed)
			time.sleep( duration )
			index -= 1
	
	def vertical_motion(self, altitude, end_x, end_y, end_z):
		self._goto_altitude(altitude)
		self._x, self._y, self._z, = self._move_smootly(self._x, self._y, end_x, end_y, altitude)
		#self._x, self._y, self._z = end_x + self.OFFSET_X - 2, end_y, altitude
		self._goto_altitude(end_z)
	
	def _goto_altitude(self, altitude):
		""" move pos.z to altitude """
		while 1:
			if abs(self._z - altitude) < self.TOLERANCE: break
			diff = altitude - self._z 
			if abs(diff) <= self.DISTANCE:
				# snap to altitude
				self._z = altitude
			else:
				# move self.DISTANCE units ..
				if altitude > self._z: self._z += self.DISTANCE
				else: self._z -= self.DISTANCE
			duration = self.send_move(self.SPEED)
			time.sleep(duration)
	
	MOVEABLE_JOINTS = [0, 1, 2, 3,]
	UNIT_TIME = 19
	def send_move(self, speed):
		""" do self.move() with calculated joints speed-s 
			modify only joints in MOVEABLE_JOINTS
			returns duration of maneuver
		"""
		global time_speed_lookuptable
		
		speeds = [12, 12, 12, 12, 12, 12,]
		old = self._joints[:]
		
		print self._x, self._y, self._z, speeds
		new = self.calculate_joints(self._x, self._y, self._z)
		diff = [0,0,0,0,0,0,]
		
		for index in self.MOVEABLE_JOINTS: 
			diff[index] = abs(new[index] - old[index])
		
		unit = max( diff )
		index = 0 
		for angle_difference in diff:
			if angle_difference > 0.0 :
				time_to_move = ( unit / diff[index] ) * self.UNIT_TIME
				speeds[index] = int( round(time_speed_lookuptable[time_to_move]) )
			index += 1 
		
		self.move(self._x, self._y, self._z, speeds)
		#ret = 1.9 / 90.0
		#ret = 0.05
		return 0.05

	def is_completed(self):
		return self._maneuver_state.done
	
	def kill_task(self): 
		""" abort further actions
		"""
		
	
	def start_task(self, step):
		""" 
		"""
		self._task = step
	
	def run(self):
		while self._working:
			
			if self._task != None:
				
				self.change_current_position(BEGIN_X, BEGIN_Y, BEGIN_Z)
				
				if self.pack_gripper == True: 
					self.build_on(self._task.x, self._task.y, self._task.z)
					self.pack_gripper = False 
				
				elif self.pack_floor_sorage == True:
					self.execute_operation('uzmi pak')
					time.sleep(0.5)
					self.build_on(self._task.x, self._task.y, self._task.z)
					self.pack_floor_sorage = False
				
				elif self.pack_left == True:
					self.execute_operation('get left')
					#time.sleep(1.2)
					self.build_on(self._task.x, self._task.y, self._task.z)
					self.pack_left = False
				
				elif self.pack_right == True:
					self.execute_operation('get right')
					#time.sleep(1.2)
					self.build_on(self._task.x, self._task.y, self._task.z)
					self.pack_right = False
				
				self._task = None
				
			else:
			
				if self.sensor_floor_sorage == True:
				
					if self.pack_left == False:
						self.execute_operation('uzmi pak')
						self.execute_operation('store left')
						#time.sleep(1.0)
						self.pack_left = True
						
					elif self.pack_right == False:
						self.execute_operation('uzmi pak')
						self.execute_operation('store right')
						#time.sleep(1.0)
						self.pack_right = True
						
					elif self.pack_gripper == False:
						self.execute_operation('uzmi pak')
						time.sleep(0.5)
						self.pack_gripper = True
					
					elif self.sensor_floor_sorage == True:
						self.pack_floor_sorage = True
			
			time.sleep(0.1)
	
	def update_world_state(self):
		self._world.lock.acquire()
		self._world.pack_left = self.pack_left
		self._world.pack_right = self.pack_right 
		self._world.pack_gripper = self.pack_gripper 
		self._world.pack_floor_sorage = self.pack_floor_sorage 
		self._world.lock.release()
	
	def decrease_storage(self):
		self.stored -= 1 
	def increase_storage(self):
		self.stored += 1 
	
	def simulate_external_signal(self, state):
		self.sensor_floor_sorage = state
	
	def change_current_position(self,x, y, z):
		self._x, self._y, self._z = x, y, z,
	
	def dump_logs(self):
		pass

if __name__ == '__main__':
	control = ArmControl()
	time.sleep(2.3)
	control.vertical_motion( 13.5, 17.0, -5.0, 3.0 )
	#control.vertical_motion( 13.5, 0.0, 0.0, 5.0 )
	#arm.move_smootly(15, 0, 6)
	
























